Fast Copter Fences #25698

andyp1per · 2023-12-04T17:01:19Z

An attempt at checking fences quickly to avoid massive breaches

Adds support for auto-enabling fence floors on copter

Current master is fixed at 3Hz so if you are going 30m/s you will be 10m before the fence before it is even checked

andyp1per · 2023-12-04T17:06:56Z

I am testing using this script: https://gist.github.com/andyp1per/85e5e29dac031e4c93d6e83d8f71641f

Here is an example log: 00000056.zip

The copter flies through the fence at a very high speed and is about 50m past it before the fence kicks in. On the way down the fence just about kicks in before it hits the ground.

libraries/AC_Fence/AC_Fence.cpp

ArduCopter/mode.cpp

Hwurzburg · 2023-12-22T00:45:03Z

does this fix plane also? I think mine fixes both....note also that autoenable handles floor differently for each case

andyp1per · 2023-12-22T09:41:32Z

does this fix plane also? I think mine fixes both....note also that autoenable handles floor differently for each case

Well it fixes the bugs I want fixed of which there are a few. Copter is actually missing the autoenable feature which this implements amongst other things. What is the bug you think that needs fixing in plane?

Hwurzburg · 2023-12-22T15:07:57Z

Fences have the following issues:

MIN_ALT fence records breach even if floor is not enabled
MIN_ALT floor always disabled on boot irrespective FENCE_ENABLE, never gets enabled unless: autoenabled, or overt enable(GCS or switch)
MIN_ALT floor stays disabled after disable floor for landing, used by NAV_LAND, QLAND and other VTOL landings in Plane, LAND, Copter RTL
Plane automatically ignores any breach while performing a breach action. If landing occurs, MIN_ALT breach is recorded but not cleared after landing, preventing mode changes if mode change prevention is enabled (Copter allows mode changes always). This prevents mode changes after landing.
AutoEnable on arming or not using AutoEnabled never disables a floor for landing. So all the above situations which think they disable floor, wont.

Any changes to AC_Fence module without considering how it may impact Plane or future fixes for plane fence issues is a problem IMHO

Having floor always enable once MIN_ALT is reached by itself wont impact future fixes and only impacts the MIN_ALT fence, whether that fence is setup or not...but it must be able to do this after disables from any altitude, and after landing for both Copter and Plane

And CI heavily incorporates all the existing wierdness of fences, so virtually any ,even minor, changes breaks something!

IamPete1 · 2023-12-31T20:42:39Z

ArduCopter/fence.cpp

@@ -24,7 +24,8 @@ void Copter::fence_check()
    if (new_breaches) {

        if (!copter.ap.land_complete) {


Its a bit odd that we don't print if landed. Its the current behavior of course. But now it will print when the breach clears in all caces, so you can end up reporting the clear but not the breach.

ArduCopter/mode.cpp

libraries/AC_Fence/AC_Fence.cpp

IamPete1 · 2023-12-31T20:49:44Z

libraries/AC_Fence/AC_Fence.cpp

-    // @Description: 0:Disable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas.
-    // @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas
+    // @Description: 0:Disable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas. When bit 2 is set any active minimum altitude floor will be disabled.
+    // @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas, 2:Auto-disable floor fence on disarming


Why would you have this bit ever disabled? I would have thought you would also want the floor disabled when disarming if you have AutoEnable::ALWAYS_ENABLED

@IamPete1 because the tests check for the current disarmed behaviour. We can change the tests of course, but we need to be all agreed that this is the correct behaviour - because it is a change. At the moment I have added new tests for the new behaviour.

andyp1per · 2024-01-01T12:38:08Z

@Hwurzburg

MIN_ALT fence records breach even if floor is not enabled

I think this is fixed by this PR, but its possible there is something special going on in plane.

MIN_ALT floor always disabled on boot irrespective FENCE_ENABLE, never gets enabled unless: autoenabled, or overt enable(GCS or switch)

Yes, this is the main fix of this PR. Without this MIN_ALT is mostly useless on Copter.

MIN_ALT floor stays disabled after disable floor for landing, used by NAV_LAND, QLAND and other VTOL landings in Plane, LAND, Copter RTL

Can you expand on this? Current behaviour is that fence remains enable but is ignored for landing so that you cannot rearm. This PR provides an option which automatically clears the breach once you have landed so that you can rearm without rebooting.

Plane automatically ignores any breach while performing a breach action. If landing occurs, MIN_ALT breach is recorded but not cleared after landing, preventing mode changes if mode change prevention is enabled (Copter allows mode changes always). This prevents mode changes after landing.

For copter see above - I think this is partially fixed (second part). I'm not sure that the first part is the wrong behavior - once your breach action has engaged you want it to stay engaged. I did fix a bug which preventing scripting seeing new breaches.

AutoEnable on arming or not using AutoEnabled never disables a floor for landing. So all the above situations which think they disable floor, wont.

I don't quite get this one but I think its plane specific as auto enable is not currently supported on copter without this PR. With this PR I am able to land ok (at least in RTL).

Hwurzburg · 2024-01-01T14:25:34Z

@andyp1per any change to AC_Fences must be THOROUGHLY tested for impacts on Plane....it took me two days of SITL sims on just MIN_ALT fence to find all the hidden anomalies...

andyp1per · 2024-01-01T16:05:33Z

@andyp1per any change to AC_Fences must be THOROUGHLY tested for impacts on Plane....it took me two days of SITL sims on just MIN_ALT fence to find all the hidden anomalies...

Sure. The solution here, of course, is making sure that the autotests cover the edge cases. I have added some tests but more can be added - be good to know what tests you think are missing. I also have spent a long time flying this in RF, that also enabled me to uncover many of these hidden "gems". Clearly the fencing code is not as widely used as we suspect otherwise they would have been revealed sooner.

libraries/AC_Fence/AC_Fence.cpp

tridge · 2024-01-02T00:05:24Z

ArduCopter/Copter.cpp

@@ -147,6 +147,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {

    SCHED_TASK(rc_loop,              250,    130,  3),
    SCHED_TASK(throttle_loop,         50,     75,  6),
+#if AP_FENCE_ENABLED
+    SCHED_TASK(fence_check,           25,    100,  7),


might be worth moving fence checking to an IO callback, or add a FENCE_OPTIONS bit for fast fence checking

libraries/AC_Fence/AC_Fence.cpp

rmackay9 · 2024-01-02T00:07:33Z

We discussed at length on the dev call but disagreed about how fast the fence check could be run. Various suggestions came up re adding a compile time update rate parameter, a parameter option, dependent upon the CPU time etc.

I think it would be good to first get a baseline of how expensive the fence check is. For example, how complex can the fence be (e.g. how many polygon points) and still complete within 1ms on an F4 CPU?

andyp1per · 2024-01-02T20:08:14Z

Fence fixes split out into #25875

andyp1per · 2024-02-26T15:33:49Z

On a CubeBlack I get about 10us per cycle for general fence checks, if I add a poly fence with 25 points It goes to about 22us percycle, if I increase this to include an exclusion fence with another 42 points I get about 33us. So being ultra conservative lets say we can get 1 point per micro that means we can have a 1000 point fence for 1ms on an F4, but in all likelihood considerably more than this. Not sure what this means for my update rate, but I think we can go a decent amount faster than 3Hz.

rmackay9

Thanks for the testing. Personally I'd make it 10hz because of the law of diminishing returns but it's probably fine at 25hz too. A very fast copter flying at 20m/s will get 6.6m breach currently, only 2m at 10hz, 0.8m at 25hz.

…e fence breaches

andyp1per · 2024-02-28T08:30:30Z

Maybe for 4.5.1 if the other fence PR gets finally approved

andyp1per added the Experiment label Dec 4, 2023

andyp1per requested a review from rmackay9 December 4, 2023 17:07

tridge reviewed Dec 6, 2023

View reviewed changes

libraries/AC_Fence/AC_Fence.cpp Outdated Show resolved Hide resolved

andyp1per force-pushed the pr-fast-fences branch from 996dae8 to d427c54 Compare December 8, 2023 16:22

peterbarker reviewed Dec 13, 2023

View reviewed changes

ArduCopter/mode.cpp Outdated Show resolved Hide resolved

andyp1per mentioned this pull request Dec 21, 2023

Improve function of MIN_ALT fence #25811

Closed

andyp1per force-pushed the pr-fast-fences branch from 5cee51e to 35db0a8 Compare December 21, 2023 20:56

andyp1per marked this pull request as ready for review December 21, 2023 20:58

andyp1per added BUG DevCallTopic and removed Experiment labels Dec 21, 2023

andyp1per force-pushed the pr-fast-fences branch from 35db0a8 to 340634a Compare December 21, 2023 20:59

andyp1per mentioned this pull request Dec 21, 2023

Enhanced attitude control via scripting #25814

Open

andyp1per force-pushed the pr-fast-fences branch from 5d55085 to c6421f5 Compare December 22, 2023 12:32

IamPete1 reviewed Dec 31, 2023

View reviewed changes

ArduCopter/mode.cpp Outdated Show resolved Hide resolved

IamPete1 reviewed Dec 31, 2023

View reviewed changes

libraries/AC_Fence/AC_Fence.cpp Outdated Show resolved Hide resolved

IamPete1 reviewed Dec 31, 2023

View reviewed changes

libraries/AC_Fence/AC_Fence.cpp Outdated Show resolved Hide resolved

IamPete1 reviewed Dec 31, 2023

View reviewed changes

andyp1per force-pushed the pr-fast-fences branch from 43c5644 to 1a0c9ad Compare January 1, 2024 12:27

andyp1per requested a review from IamPete1 January 1, 2024 13:39

tridge requested changes Jan 2, 2024

View reviewed changes

tridge removed the DevCallTopic label Jan 2, 2024

andyp1per mentioned this pull request Jan 2, 2024

Copter Fence Fixes #25875

Merged

andyp1per force-pushed the pr-fast-fences branch from 1a0c9ad to 99f020e Compare January 2, 2024 19:55

andyp1per force-pushed the pr-fast-fences branch from 99f020e to bd98e13 Compare February 26, 2024 14:36

rmackay9 approved these changes Feb 26, 2024

View reviewed changes

andyp1per requested a review from tridge February 27, 2024 11:52

Copter: check fence at faster rates when going faster to avoid massiv…

04e2877

…e fence breaches

andyp1per force-pushed the pr-fast-fences branch from bd98e13 to 04e2877 Compare February 27, 2024 22:40

andyp1per added the DevCallEU label Feb 28, 2024

tridge approved these changes Feb 28, 2024

View reviewed changes

tridge merged commit f0b691d into ArduPilot:master Feb 28, 2024
72 checks passed

andyp1per deleted the pr-fast-fences branch February 28, 2024 08:29

andyp1per removed the DevCallEU label Feb 28, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fast Copter Fences #25698

Fast Copter Fences #25698

andyp1per commented Dec 4, 2023 •

edited

Loading

andyp1per commented Dec 4, 2023

Hwurzburg commented Dec 22, 2023 •

edited

Loading

andyp1per commented Dec 22, 2023

Hwurzburg commented Dec 22, 2023 •

edited

Loading

IamPete1 Dec 31, 2023

IamPete1 Dec 31, 2023

andyp1per Jan 1, 2024 •

edited

Loading

andyp1per commented Jan 1, 2024

Hwurzburg commented Jan 1, 2024

andyp1per commented Jan 1, 2024

tridge Jan 2, 2024

rmackay9 commented Jan 2, 2024

andyp1per commented Jan 2, 2024

andyp1per commented Feb 26, 2024 •

edited

Loading

rmackay9 left a comment

andyp1per commented Feb 28, 2024

		@@ -24,7 +24,8 @@ void Copter::fence_check()
		if (new_breaches) {

		if (!copter.ap.land_complete) {

Fast Copter Fences #25698

Fast Copter Fences #25698

Conversation

andyp1per commented Dec 4, 2023 • edited Loading

andyp1per commented Dec 4, 2023

Hwurzburg commented Dec 22, 2023 • edited Loading

andyp1per commented Dec 22, 2023

Hwurzburg commented Dec 22, 2023 • edited Loading

IamPete1 Dec 31, 2023

Choose a reason for hiding this comment

IamPete1 Dec 31, 2023

Choose a reason for hiding this comment

andyp1per Jan 1, 2024 • edited Loading

Choose a reason for hiding this comment

andyp1per commented Jan 1, 2024

Hwurzburg commented Jan 1, 2024

andyp1per commented Jan 1, 2024

tridge Jan 2, 2024

Choose a reason for hiding this comment

rmackay9 commented Jan 2, 2024

andyp1per commented Jan 2, 2024

andyp1per commented Feb 26, 2024 • edited Loading

rmackay9 left a comment

Choose a reason for hiding this comment

andyp1per commented Feb 28, 2024

andyp1per commented Dec 4, 2023 •

edited

Loading

Hwurzburg commented Dec 22, 2023 •

edited

Loading

Hwurzburg commented Dec 22, 2023 •

edited

Loading

andyp1per Jan 1, 2024 •

edited

Loading

andyp1per commented Feb 26, 2024 •

edited

Loading